/**
 * @file Compensate.cpp
 * @author enemy1205 (enemy1205@qq.com)
 * @brief 补偿类
 * @date 2021-10-10
 */
#include "Compensate.h"

/**
 * @brief 构造函数初始化
 */
Compensate::Compensate() {
    this->flying_time = Other_param.fly_time;
    this->xy_compensate = {0, 0};
}

/**
 * @brief 计算真实的 y 坐标
 *
 * @param distance 相机到装甲板的水平距离 (m)
 * @param velocity 枪口射速 (m/s)
 * @param angle 枪口与水平方向的夹角 (rad)
 *
 * @return 世界坐标系下真实的 y 坐标
 */
float Compensate::BulletModel(float distance, float velocity, float angle) {
    // x(m), v(m/s), angle(rad)
    // 忽略空气阻力影响
    this->flying_time = distance / (velocity * cosf(angle));
    return velocity * sinf(angle) * this->flying_time - g * this->flying_time * this->flying_time / 2;
}

/**
 * @brief 获得补偿角度
 *
 * @param x 目标离相机的水平宽度
 * @param y 目标离相机的铅垂高度
 * @param velocity 枪口射速
 *
 * @return 补偿角度
 */
float Compensate::GetPitch(float x, float y, float velocity) {
    float y_temp, y_actual, dy;
    float angle = 0.f;
    y_temp = y;
    // 使用迭代法求得补偿角度
    for (int i = 0; i < compensateParam.ITERATE_TIMES; i++) {
        angle = (float) atan2(y_temp, x);
        y_actual = BulletModel(x, velocity, angle);
        dy = y - y_actual;
        y_temp = y_temp + dy;
        if (fabsf(dy) < compensateParam.ITERATE_ACCURACY) {
            break;
        }
    }
    return angle;
}

/**
 * @brief 补偿类对外接口
 * @param tracker 待补偿追踪器对象
 * @param gyroData 最新陀螺仪数据
 */
void Compensate::compensate(tracker_ptr &tracker, const GyroData_ptr &gyroData) {
    float angle = -gyroData->pitch + tracker->getGoalFeature()->getRelativeAngles().y;//哨兵pitch以下为正
    float x_com = compensateParam.YAW_COMPENSATE;
    float y_com = Rad2Deg(GetPitch(tracker->getPnp().distance / 1000.f,
                                   angle + compensateParam.PITCH_COMPENSATE,
                                   30) - angle);
    // 获取 pitch、yaw 补偿值
    this->xy_compensate = Point2f(x_com, y_com);
}